/****************************************************************************
- Copyright (c) 2021, bai-yangyang
- File name : main.c
- Author : Bai-yangyang				Version:3.0				Data:2023.06.29
- Description:   nano msg/ update/ log
- Others :config
- Function List:
  1、......
  2、......
- History :
  1、Data:2023.0629
     Author:byy
     Modification:1.log md5 2.std::shared_ptr
  2、
******************************************************************************/
#include "serial_port.h"
#include "aulCrc.h"
#include "nano_tcp.h"
#include "project_path.h"
#include "zip_pt.h"
#include "fileManage.h"
#include "openssl_md5.h"

using namespace std;
bool PoseBit = false, ImuBit = false, SyncTimeBit = false;
int PrtTmn = 0, realsens_ctl = 0;
int time_system = 0;
float deceleration_ratio, wheel_interval, multiplier, wheel_diameter, line_number;
double imu_last_time = 0;
SqQueue Q_Save; // 接收循环队列
MLog *mlog;     // LOG文件
ofstream Out_Pose_dirStream, Imu_Pose_dirStream;
nano_tcp tcp_fst;
ros::Time sensor_time;
position now_pos;
deque<position> PosList;
Mytime ObjTime;
// a4 59 43 03 22 01 00 00 00 4d aa 55 00 00 00 00 00 a2 16 01 00 bb 66 11 e9 cd 18 48 1a ee 65 c1 c0 8c e7 39 36 78 4a c1 00 00 00 00 00 00 00 00 00 00 00 00 00 cc 77 22 c7 ff 5b 3f 57 92 46 3f 2f c6 19 41 64 46 1b bd 9d b1 c6 3e ff 7f 3b 3d dd 88 33 00 00 00 00 0d 0a// a4 59 43 ai(04) fuction_main function_pub length_a length_b length_c length_d ...... 0d 0a
uint8_t send_data_hand[] = {0xA4, 0x59, 0x43, 0x04, 0x07, 0x02, 0x00, 0x00, 0x00, 0x01, 0x00, 0x0D, 0x0A};   // 0x00 0x00 0x00 0x01 长度为1
uint8_t send_data_update[] = {0xA4, 0x59, 0x43, 0x04, 0x14, 0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x0D, 0x0A}; // 0x00 0x00 0x00 0x01 长度为1
uint8_t send_data_buf4[] = {0xa4, 0x59, 0x43, 0x04, 0x58, 0x01, 0x00, 0x00, 0x00, 0x04, 0x00, 0x00, 0xE0, 0xF2, 0x0D, 0x0A};
uint8_t send_data_map[] = {0xA4, 0x59, 0x43, 0x04, 0x08, 0x01, 0x00, 0x00, 0x00, 0x15, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0D, 0x0A}; // 0x00 0x00 0x00 0x01 长度为1
uint8_t send_buf_1[] = {0xA4, 0x59, 0x43, 0x04, 0xFF, 0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x0D, 0x0A};                                                                                                                            // 0x00 0x00 0x00 0x01 长度为1
uint8_t send_buf_2[] = {0xA4, 0x59, 0x43, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x0D, 0x0A};                                                                                                                      // 0x00 0x00 0x00 0x01 长度为1
uint8_t idle_modle = 0x02;
uint8_t send_date_Head[] = {
    0xA4, 0x59, 0x43, 0x04,                         // begin 0-3
    0x01, 0x01,                                     // 01 function 4-5
    0x00, 0x00, 0x00, 0x1C,                         // length 6-9
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // time 10-17
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // state 18-25
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // error 26-33
    0x00, 0x00, 0x00, 0x00,                         // rtk_x 34-37
    0x00, 0x00, 0x00, 0x00,                         // rtk_y 38-41
    0x00, 0x00, 0x00, 0x00,                         // rtk_z 42-45
    0x00, 0x00, 0x00, 0x00,                         // x     46-49
    0x00, 0x00, 0x00, 0x00,                         // y     50-53
    0x00, 0x00, 0x00, 0x00,                         // z     54-57
    0x00, 0x00, 0x00, 0x00,                         // roll  58-61
    0x00, 0x00, 0x00, 0x00,                         // pitch 62-65
    0x00, 0x00, 0x00, 0x00,                         // yaw   66-69
    0x0D, 0x0A};                                    // 70-71
/****************************************************************************
- Function :        SerialPub
- Description :     config set
- Calls :           subscribe
- Callded By:       main.cpp
- Input:            node
- Output:           void
- Return:           void
- Others :          null
******************************************************************************/
SerialPub::SerialPub(ros::NodeHandle node) {
    RED_CFG();
    timesync_client_init("192.168.10.150", "192.168.10.110", 2000, 2000);         // 时间同步初始化
    mlog = MLog::logCntl(1, m_log_.c_str(), 5 * 1024 * 1024, 1, Priority::DEBUG); // 5*1024*1024=5M
    mlog->logInfo(
        "\n"
        "********************************************************************"
        "\n"
        "********************************************************************"
        "\n"
        "********************************************************************"
        "\n"
        "*****************************Serial Version ************************"
        "\n"
        "********************************date:2023.07.13*********************"
        "\n"
        "********************************************************************"
        "\n"
        "********************************************************************"
        "\n"
        "********************************************************************"
        "\n");
    OPEN_CMNCT(UsbName.c_str());
    CldPotAiSub = node.subscribe("/cloud_point_ai", 5, &SerialPub::PCA_CBK, this);
    LocationSub = node.subscribe("/Location_pose_by", 5, &SerialPub::LOC_CBK, this);
    UpDownSub = node.subscribe("/Updown_car", 5, &SerialPub::UP_DWN_CBK, this);
    LocSttSub = node.subscribe("/Location_state", 5, &SerialPub::LOC_STT_CBK, this);
    AiSttSub = node.subscribe("/Ai_State", 5, &SerialPub::AI_STT_CBK, this);
    FixGpsPub = node.advertise<sensor_msgs::NavSatFix>("GPS_fix", 20);
    TwsOdomPub = node.advertise<geometry_msgs::TwistStamped>("Ser_odom", 20);
    SensImuPub = node.advertise<sensor_msgs::Imu>("Ser_imu", 20);
    SndToAiPub = node.advertise<std_msgs::UInt8MultiArray>("Usb_Ai", 20);
    SndToSlmPub = node.advertise<std_msgs::UInt8MultiArray>("Usb_Slam", 20);
    if (PoseBit) {
        SAV_DATA(Out_Pose_dirStream, pst_sv);
    }
    if (ImuBit) {
        SAV_DATA(Imu_Pose_dirStream, imu_sv);
    }
    InitQueue(Q_Save);
    INTLZ_CONFIG();
    if (SyncTimeBit)
        DataSyncTimeThrd = std::thread(&SerialPub::PPS_RCV, this);
    DataSttThrd = std::thread(&SerialPub::SYS_STT, this);
    DataDelThrd = std::thread(&SerialPub::DEAL_DATA, this);
    DataRcvThrd = std::thread(&SerialPub::DATA_RCV, this); /* */
    DataSndThrd = std::thread(&SerialPub::DATA_SND, this);
    LogSendThrd = std::thread(&SerialPub::BLOCK_THREAD, this);
}

SerialPub::~SerialPub() {
    delete my_time;
    MLog::logCntl(0, " ");
    close(device);
    close(up_file);
    if (PoseBit)
        Out_Pose_dirStream.close();
    if (ImuBit)
        Imu_Pose_dirStream.close();
}
void SAV_DATA(ofstream &drstm, string &pst_) {
    drstm.open(pst_, std::ios::out);
    drstm.close();
    drstm.open(pst_, std::ios::app);
    drstm.setf(std::ios::fixed, std::ios::floatfield);
    drstm.precision(9);
}

void MSG_PRINTF(const std::string &msg) {
    timeval now;
    char msg_s[100] = {0};
    gettimeofday(&now, NULL);
    sprintf(msg_s, "%ld.%06ld:%s", now.tv_sec, now.tv_usec, msg.c_str());
    printf("%s\n", msg_s);
}

void MSG_PRINTF_FLOAT(const std::string &msg_a, float msg) {
    timeval now;
    char msg_s[100] = {0};
    gettimeofday(&now, NULL);
    sprintf(msg_s, "%ld.%06ld:%s %f", now.tv_sec, now.tv_usec, msg_a.c_str(), msg);
    printf("%s\n", msg_s);
}

void MSG_STATE(int bit, const std::string &msg_a) {
    timeval now;
    char msg_s[100] = {0};
    gettimeofday(&now, NULL);
    sprintf(msg_s, "%ld.%06ld:%s", now.tv_sec, now.tv_usec, msg_a.c_str());
    if (bit == 2)                                  // error
        printf("\033[1;31m %s \033[0m \n", msg_s); // error
    else if (bit == 1)
        printf("\033[1;33m %s \033[0m \n", msg_s); // warnning
    else
        printf("\033[1;32m %s \033[0m \n", msg_s); // state
}

void SerialPub::BLOCK_THREAD() {
    while (ros::ok()) {
        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
        if (RecState.detail.Loging_Px30) {
            if (log_mode == 1) {
                removeFileAndDirectory(log_pkg_img.c_str());
                MKDIR(log_pkg_img.c_str());
            }
            const char *mlog_pkg = log_pkg.c_str();
            const char *mlog_pkg_zip = log_pkg_zip.c_str();
            mlog->logInfo("Tx2 back message************Serial :begin remove zip");
            removeFileAndDirectory(log_pkg_zip);
            mlog->logInfo("Tx2 back message************Serial :begin remove over,begin zip");
            zip_file_pt(mlog_pkg, mlog_pkg_zip); // 打包
            mlog->logInfo("Tx2 back message************Serial :zip over");
            sleep(5);
            getMd5File(mlog_pkg_zip, log_md5_result);
            printf("md5 num %s\n", log_md5_result);
            std::string command = "nc -w 3 192.168.10.150 918 < " + common_path + "robot_logs.zip";
            mlog->logInfo("Tx2 back message************Serial :begin nc");
            system((char *)command.c_str());
            mlog->logInfo("Tx2 back message************Serial :nc over");
            sleep(5);
            mlog->logInfo("Tx2 back message************Serial :sleep 30 send log over");
            RecState.detail.Loging_Px30 = false;
            RecState.detail.Log_Px30 = true;
        }
        if (RecState.detail.DownCaring) {
            if (realsens_ctl == 1)
                rosnode_manger("roslaunch realsense2_camera rs_camera.launch&");
            RecState.detail.DownCaring = false;
        }
        if (RecState.detail.FinCaring) {
            if (realsens_ctl == 1) {
                rosnode_manger("rosnode kill /camera/realsense2_camera&");
                rosnode_manger("rosnode kill /camera/realsense2_camera_manager&");
            }
            RecState.detail.FinCaring = false;
        }
    }
}
void SerialPub::SET_SYSTEM_TIME() {
    timeval stime;
    gettimeofday(&stime, NULL);
    printf("现在的时间秒数是：%ld,毫秒数是：%ld\n现在的时间是:", stime.tv_sec, stime.tv_usec);
    fflush(stdout);
    system("date"); // 向shell发送一条命令，查询日期
    stime.tv_sec = time_daysec;
    stime.tv_usec = time_daynsec / 1000;
    settimeofday(&stime, NULL);
    printf("现在的时间秒数是：%ld,毫秒数是：%ld\n现在的时间是:", stime.tv_sec, stime.tv_usec);
    fflush(stdout);
    system("date");
}
void SerialPub::RED_CFG() {
    string yaml_path = (string)SERIAL_PATH + (string) "/config/config.yaml"; // 需要编译生成serial_date path
    common_path = readConfig(yaml_path, "common_path", (string) "/home/nvidia/positec/");
    ver_path = common_path + "robot/misc/version";
    log_pkg = common_path + "robot_logs";
    log_pkg_img = common_path + "robot_logs/img";
    log_pkg_zip = common_path + "robot_logs.zip";
    robot_fm_pkg = common_path + "robot/src/serial_date/shell/replace_fm.sh";
    time_system = readConfigInt(yaml_path, "time_system", 1); // 时间来自与系统还是px30，1为系统，0为px30
    sub_addr = readConfig(yaml_path, "sub_addr", (string) "10.0.0.1");
    pub_addr = readConfig(yaml_path, "pub_addr", (string) "10.0.0.1");
    UsbName = readConfig(yaml_path, "usb_name", (string) "/dev/ttyGS1");
    m_log_ = common_path + readConfig(yaml_path, "log_path", (string) "robot_logs/robot_log.log");
    pst_sv = common_path + readConfig(yaml_path, "PostSave", (string) "robot_logs/position_log.log");
    imu_sv = common_path + readConfig(yaml_path, "ImuSave", (string) "robot_logs/imu_log.log");
    rtk_path_ = common_path + readConfig(yaml_path, "rtk_path", (string) "robot/src/Global-Sensor-Fusion/config/rtk_init.txt");
    update_path_ = common_path + readConfig(yaml_path, "update_path", (string) "robot_update/tx2_update.tar.gz.enc");
    PoseBit = readConfigInt(yaml_path, "Post_Bit", 0);
    ImuBit = readConfigInt(yaml_path, "Imu_Bit", 0);
    PrtTmn = readConfigInt(yaml_path, "printf_terminal", 0);
    realsens_ctl = readConfigInt(yaml_path, "realsense_ctl", 0);
    SyncTimeBit = readConfigInt(yaml_path, "time_sync", 0);
    deceleration_ratio = readConfigFloat(yaml_path, "deceleration_ratio", 0.0f); // 减速比
    wheel_interval = readConfigFloat(yaml_path, "wheel_interval", 0.0f);         // 两轮轴距mm
    multiplier = readConfigFloat(yaml_path, "multiplier", 0.0f);                 // 倍频数
    wheel_diameter = readConfigFloat(yaml_path, "wheel_diameter", 0.0f);         // 轮子直径，单位mm
    line_number = readConfigFloat(yaml_path, "line_number", 0.0f);
}

void SerialPub::INTLZ_CONFIG() {
    my_time = new TIME[1];
    upgrade_length = 0;
    sync_send_bit = 0;
    device = -1;
    up_file = -1;
    pps_bit = false;
    handshake_bit = false;
    RecState.state = 0;
    RecState.detail.Ver_Px30 = true; // 开机发送版本
    odom_X = 0;
    odom_Y = 0;
    Yaw_Angle = 0;
    tx2_error.state = 0;
    UsbStt = true;
    memset(vec_loc, 0, sizeof(vec_loc)); // 只能初始化值为0/-1
    memset(frame_buff_, 0, sizeof(frame_buff_));
    memset(buf_save, 0, sizeof(buf_save));
    dir_cal = 0;
    rcv_alg_slm_state.state = 0x00;
    snd_slm_state.state = 0x00;
    rcv_ai_state.state = 0x00;
    snd_ai_state.state = 0x00;
    map_service = 0x00;
    for (int i = 0; i < 10; i++)
        alg_error_time_.buf[i] = 0x00;
    rtk_x = 0;
    rtk_y = 0;
    rtk_z = 0;
    sync_success_bit = -1;
    last_sync_bit = -1;
    Px30_Mode_Contril.state = 0;
    one_secend_count.header_tik = 0;
    one_secend_count.imu_tik = 0;
    one_secend_count.obj_tik = 0;
    one_secend_count.odom_tik = 0;
    one_secend_count.pos_tik = 0;
    one_secend_count.rtk_tik = 0;
    one_secend_count.temp_tik = 0;
    one_secend_count.error_tik = 0;
    vec_ai.push_back(0x01);
    vec_ai.push_back(0x00);
    vec_ai.push_back(0x00);
    AiUint8mltBuf.data.resize(len_usb_ai);
    SlamUint8mltBuf.data.resize(len_usb_slam);
    const_frame = wheel_diameter * pi / (line_number * multiplier * deceleration_ratio) / 1000.0f; // unit m,pi*d/线数/预分频/减速比   / = 一个编码器值为多长
    const_angle = const_frame * 1000.0f / wheel_interval;                                          // 角度=上式/轮距	   // 一般在小于5度的时候   角度值=sin(角度)=tan(角度)=x/y;
    const_speed = line_number * multiplier * deceleration_ratio / pi / wheel_diameter;
}

void SerialPub::OPEN_CMNCT(const char *c_s) {
    while (ros::ok()) {
#ifdef NANO_RUN
        if (!(tcp_fst.nano_tcp_pub_init(pub_addr.c_str()) && tcp_fst.nano_tcp_sub_init(sub_addr.c_str())) && ros::ok()) {
            std::this_thread::sleep_for(std::chrono::milliseconds(3000));
            mlog->logError("Serial Program:unable to open NET");
        } else {
            mlog->logInfo("Serial Program:NET opened successfully");
            break;
        }
#else
        device = open_serial_port(c_s, 230400);
        if (device < 0) {
            mlog->logError("Serial Program:unable to open USB");
            std::this_thread::sleep_for(std::chrono::milliseconds(3000));
        } else {
            mlog->logInfo("Serial Program:USB opened successfully");
            std::this_thread::sleep_for(std::chrono::milliseconds(1000));
            break;
        }
#endif
    }
}

void CP_FILE(const char *sourcefile, const char *destfile) {
    std::string command = "cp ";
    command += sourcefile;
    command += " ";
    command += destfile;
    system((char *)command.c_str());
}

void SYS_RESTART_PTP() {
    std::string command = "systemctl start ptp4l.service";
    std::cout << command << std::endl;
    system((char *)command.c_str()); //
}

void TMN_PRT(uint8_t *buf, int buf_size) {
    for (int ss_i = 0; ss_i < buf_size; ss_i++) {
        printf("%02x ", buf[ss_i]);
    }
    printf("\n");
}
string rosnode_manger(string function) {
    FILE *fp;
    char buff[20];
    string ret_str = "";
    fp = popen(function.c_str(), "r");
    fgets(buff, sizeof(buff), fp);
    ret_str = buff;
    std::cout << function << " :" << ret_str << std::endl;
    pclose(fp);
    return ret_str;
}
int send_log = -1;
void SerialPub::DATA_SND() {
    std::vector<uint8_t> vec;
    uint32_t send_length = 0x01;
    uint64_t out_time = 0;
    bool Ai_Location = false;
    float save_pose[3] = {0};
    while (ros::ok()) {
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
        Ai_Location = !Ai_Location;
        if (send_log > -1) {
            send_log++;
            handshake_bit = true;
        }
        if (handshake_bit && (usb_st != -1)) {
            vec.resize(len_all);
            if (RecState.detail.Location_bit && Ai_Location) {
                PoseSndMutex.lock();
                memcpy(&vec[len_head_loc], &vec_loc[8], len_loc);
                PoseSndMutex.unlock();
                snd_slm_state.state = rcv_alg_slm_state.state; // 算法传过来的原始值
                snd_ai_state.state = rcv_ai_state.state;
                if (SlamUint8mltBuf.data[1] == 0x01) // 对slam状态进行修改，航向校正
                {
                    snd_slm_state.detail.thd = 0x01;
                }
                if (SlamUint8mltBuf.data[2] != 0x00) // 上下车状态
                {
                    snd_slm_state.detail.four = 0x06;
                }
                RecState.detail.Location_bit = false;
                for (int i = 0; i < len_head + len_fun; i++) {
                    vec[i] = send_date_Head[i];
                } // head
                vec[4] = 0x02;
                vec.push_back(send_date_Head[sizeof(send_date_Head) - 2]);
                vec.push_back(send_date_Head[sizeof(send_date_Head) - 1]);

                send_length = vec.size() - len_head_data - len_end;
                vec[6] = (send_length >> 24) & 0xFF;
                vec[7] = (send_length >> 16) & 0xFF;
                vec[8] = (send_length >> 8) & 0xFF;
                vec[9] = send_length;
                memcpy(&vec[10], &vec_loc[0], 8);
                vec[18] = tx2_error.Detail.U7;
                vec[19] = tx2_error.Detail.U6;
                vec[20] = tx2_error.Detail.U5;
                vec[21] = tx2_error.Detail.U4;
                vec[22] = tx2_error.Detail.U3;
                vec[23] = tx2_error.Detail.U2;
                vec[24] = tx2_error.Detail.U1;
                vec[25] = tx2_error.Detail.U0;

                vec[26] = snd_slm_state.detail.fst;
                vec[27] = snd_slm_state.detail.sec.astate;
                vec[28] = snd_slm_state.detail.thd;
                vec[29] = snd_slm_state.detail.four;
                vec[30] = snd_ai_state.detail.fst;
                vec[31] = snd_ai_state.detail.sec.astate;
                vec[32] = snd_ai_state.detail.thd;
                vec[33] = snd_ai_state.detail.four;

                uint8_t send_buf[vec.size()];
                // 修改
                memcpy(&send_buf[0], &vec[0], vec.size());
                if (PrtTmn == 1)
                    MSG_STATE(0, "LOC UPDATE");
                if (PrtTmn == 1) {
                    for (int send_i = 0; send_i < vec.size(); send_i++) {
                        printf("%02x ", send_buf[send_i]);
                    }
                    printf("\n");
                }
                if (mlog_save_tik.detail.position) {
                    float snd_x, snd_y, snd_yaw;
                    memcpy(&snd_x, &vec[len_head_loc + 12], 4);
                    memcpy(&snd_y, &vec[len_head_loc + 16], 4);
                    memcpy(&snd_yaw, &vec[len_head_loc + 32], 4);
                    mlog->logInfo("send location:" + to_string(snd_x) + "," + to_string(snd_y) + "," + to_string(snd_yaw));
                    mlog_save_tik.detail.position = false;
                }
#ifdef NANO_RUN
                tcp_fst.nano_tcp_send(send_buf, sizeof(send_buf));
#else
                usb_st = write_port(device, send_buf, sizeof(send_buf));
#endif
            } else if (RecState.detail.AiCloud_bit) {
                PoseSndMutex.lock();
                while (PosList.size() > 20) {
                    PosList.pop_front();
                }
                int LocObjNum = 0;
                for (int i = 0; i < PosList.size(); i++) {
                    if (PosList[i].pos.time > ObjTime.time) {
                        if (i == 0) {
                            LocObjNum = 0;
                        } else {
                            if (PosList[i].pos.time - ObjTime.time < ObjTime.time - PosList[i - 1].pos.time)
                                LocObjNum = i;
                            else
                                LocObjNum = i - 1;
                        }
                        break;
                    }
                }
                memcpy(&vec[len_head_loc], &PosList[LocObjNum].bit[8], 36); // 位置
                memcpy(&vec[10], &PosList[LocObjNum].bit, 8);               // 时间
                PoseSndMutex.unlock();
                AiSndMutex.lock();
                vec.insert(vec.end(), vec_ai.begin(), vec_ai.end()); // 插入AI数据
                AiSndMutex.unlock();
                snd_slm_state.state = rcv_alg_slm_state.state; // 算法传过来的原始值
                snd_ai_state.state = rcv_ai_state.state;
                if (SlamUint8mltBuf.data[1] == 0x01) // 对slam状态进行修改，航向校正
                {
                    snd_slm_state.detail.thd = 0x01;
                }
                if (SlamUint8mltBuf.data[2] != 0x00) // 上下车状态
                {
                    snd_slm_state.detail.four = 0x06;
                }
                RecState.detail.AiCloud_bit = false;
                for (int i = 0; i < len_head + len_fun; i++) {
                    vec[i] = send_date_Head[i];
                } // head
                vec.push_back(send_date_Head[sizeof(send_date_Head) - 2]);
                vec.push_back(send_date_Head[sizeof(send_date_Head) - 1]);

                send_length = vec.size() - len_head_data - len_end;
                vec[6] = (send_length >> 24) & 0xFF;
                vec[7] = (send_length >> 16) & 0xFF;
                vec[8] = (send_length >> 8) & 0xFF;
                vec[9] = send_length;

                vec[18] = tx2_error.Detail.U7;
                vec[19] = tx2_error.Detail.U6;
                vec[20] = tx2_error.Detail.U5;
                vec[21] = tx2_error.Detail.U4;
                vec[22] = tx2_error.Detail.U3;
                vec[23] = tx2_error.Detail.U2;
                vec[24] = tx2_error.Detail.U1;
                vec[25] = tx2_error.Detail.U0;

                vec[26] = snd_slm_state.detail.fst;
                vec[27] = snd_slm_state.detail.sec.astate;
                vec[28] = snd_slm_state.detail.thd;
                vec[29] = snd_slm_state.detail.four;
                vec[30] = snd_ai_state.detail.fst;
                vec[31] = snd_ai_state.detail.sec.astate;
                vec[32] = snd_ai_state.detail.thd;
                vec[33] = snd_ai_state.detail.four;

                uint8_t send_buf[vec.size()];
                // 修改
                memcpy(&send_buf[0], &vec[0], vec.size());
                if (PrtTmn == 2)
                    MSG_STATE(0, "AI UPDATE");
                if (PrtTmn == 2) {
                    for (int send_i = 0; send_i < vec.size(); send_i++) {
                        printf("%02x ", send_buf[send_i]);
                    }
                    printf("\n");
                }
                if (mlog_save_tik.detail.object) {
                    mlog->logInfo("send object " + to_string(send_buf[72]));
                    mlog_save_tik.detail.object = false;
                }
#ifdef NANO_RUN
                tcp_fst.nano_tcp_send(send_buf, sizeof(send_buf));
#else
                usb_st = write_port(device, send_buf, sizeof(send_buf));
#endif
                if (PoseBit) {
                    double serial_tnow = (ros::Time::now()).toSec();
                    save_pose[0] = *(float *)&send_buf[len_head_loc + 12];
                    save_pose[1] = *(float *)&send_buf[len_head_loc + 16];
                    save_pose[2] = *(float *)&send_buf[len_head_loc + 20];
                    Out_Pose_dirStream << serial_tnow << " "
                                       << save_pose[0] << " "
                                       << save_pose[1] << " "
                                       << save_pose[2] << std::endl;
                }
                if (usb_st == 0 && PrtTmn) {
                    MSG_PRINTF("serial send success");
                    std::cout << "send length=" << send_length << std::endl;
                }
            } else if (RecState.detail.Updown_bit) // if为true则不执行剩下的else if
            {
                vec.resize(len_head_data);
                for (int i = 0; i < len_head_data; i++) {
                    vec[i] = send_date_Head[i];
                }                     // head
                vec[4] = Tx2_p30_UDC; // 标志位
                vec[5] = 0x01;
                vec.insert(vec.end(), vec_updown.begin(), vec_updown.end());
                vec.push_back(send_date_Head[sizeof(send_date_Head) - 2]);
                vec.push_back(send_date_Head[sizeof(send_date_Head) - 1]);
                send_length = vec.size() - len_head_data - len_end;
                vec[6] = (send_length >> 24) & 0xFF;
                vec[7] = (send_length >> 16) & 0xFF;
                vec[8] = (send_length >> 8) & 0xFF;
                vec[9] = send_length;
                uint8_t send_buf[vec.size()];
                memcpy(&send_buf[0], &vec[0], vec.size());
                if (PrtTmn == 3) {
                    TMN_PRT(&send_buf[0], vec.size());
                }
#ifdef NANO_RUN
                usb_st = tcp_fst.nano_tcp_send(send_buf, sizeof(send_buf));
#else
                usb_st = write_port(device, send_buf, sizeof(send_buf));
#endif
                RecState.detail.Updown_bit = false;
            } else if (RecState.detail.sync_time_px30_tx2) {
                send_buf_1[4] = PX30_TX2_SYNC_TIME;
                send_buf_1[10] = sync_send_bit;
#ifdef NANO_RUN
                usb_st = tcp_fst.nano_tcp_send(send_buf_1, sizeof(send_buf_1));
#else
                usb_st = write_port(device, send_buf_1, sizeof(send_buf_1));
#endif
                RecState.detail.DownCar_px30 = false;
                RecState.detail.sync_time_px30_tx2 = false;
            } else if (RecState.detail.Log_Px30 || send_log > 2000) {
                mlog->logInfo("Tx2 back message************send log over");
                send_log = -1;
                // 发送LOG//打开文件//拷贝文件//读取数据
                uint8_t buf[48] = {0xa4, 0x59, 0x43, 0x04, 0x58, 0x01, 0x00, 0x00, 0x00, 0x24, 0x00, 0x00, 0xE0, 0xF2, 0x0D, 0x0A};
                const char *mlog_pkg_zip = log_pkg_zip.c_str();
                FILE *in;
                in = fopen(mlog_pkg_zip, "r"); // const char* mlog_s=m_log_.c_str();
                fseek(in, 0, SEEK_END);
                int file_length = ftell(in); // 计算大小
                fclose(in);
                std::cout << "log length =" << file_length << std::endl;
                buf[10] = (file_length >> 24) & 0xff;
                buf[11] = (file_length >> 16) & 0xff;
                buf[12] = (file_length >> 8) & 0xff;
                buf[13] = (file_length)&0xff;
                memcpy(&buf[14], &log_md5_result[0], 32);
                //TMN_PRT(&buf[0], 48);

#ifdef NANO_RUN
                usb_st = tcp_fst.nano_tcp_send(buf, sizeof(buf));
#else
                usb_st = write_port(device, buf, sizeof(buf));
#endif
                RecState.detail.Log_Px30 = false; // 如果为零表示发送完成
            } else if (RecState.detail.Dir_Cal_bit) {
                mlog->logInfo("Tx2 back message************Dir car");
                send_buf_1[4] = Tx2_p30_RB;
                send_buf_1[10] = Dir_Cal_Req;
#ifdef NANO_RUN
                usb_st = tcp_fst.nano_tcp_send(send_buf_1, sizeof(send_buf_1));
#else
                usb_st = write_port(device, send_buf_1, sizeof(send_buf_1));
#endif
                RecState.detail.Dir_Cal_bit = false;
            } else if (RecState.detail.Ui_Px30) {
                mlog->logInfo("Tx2 back message************Ui charge");
                send_buf_1[4] = Tx2_p30_UI;
                send_buf_1[10] = idle_modle;
#ifdef NANO_RUN
                usb_st = tcp_fst.nano_tcp_send(send_buf_1, sizeof(send_buf_1));
#else
                usb_st = write_port(device, send_buf_1, sizeof(send_buf_1));
#endif
                RecState.detail.Ui_Px30 = false; //RecState.detail.Send_Pic
            } else if (RecState.detail.Send_Pic) {
                mlog->logInfo("Tx2 back message************send pic");
                send_buf_1[4] = Tx2_p30_Pic;
                send_buf_1[10] = 0x03;
#ifdef NANO_RUN
                usb_st = tcp_fst.nano_tcp_send(send_buf_1, sizeof(send_buf_1));
#else
                usb_st = write_port(device, send_buf_1, sizeof(send_buf_1));
#endif
                RecState.detail.Send_Pic = false;  //RecState.detail.Send_Pic
            } else if (RecState.detail.UpCar_px30) // UpCar_px30,DownCar_px30,FinCar_px30
            {
                send_buf_1[4] = Tx2_p30_RB;
                send_buf_1[10] = UpCar_Req;
#ifdef NANO_RUN
                usb_st = tcp_fst.nano_tcp_send(send_buf_1, sizeof(send_buf_1));
#else
                usb_st = write_port(device, send_buf_1, sizeof(send_buf_1));
#endif
                RecState.detail.UpCar_px30 = false;
            } else if (RecState.detail.DownCar_px30) // UpCar_px30,DownCar_px30,FinCar_px30
            {
                send_buf_1[4] = Tx2_p30_RB;
                send_buf_1[10] = DownCar_Req;
#ifdef NANO_RUN
                usb_st = tcp_fst.nano_tcp_send(send_buf_1, sizeof(send_buf_1));
#else
                usb_st = write_port(device, send_buf_1, sizeof(send_buf_1));
#endif
                RecState.detail.DownCar_px30 = false;
            } else if (RecState.detail.FinCar_px30) // UpCar_px30,DownCar_px30,FinCar_px30 Slam_Reset
            {
                send_buf_1[4] = Tx2_p30_RB;
                send_buf_1[10] = FinCar_Req;
#ifdef NANO_RUN
                usb_st = tcp_fst.nano_tcp_send(send_buf_1, sizeof(send_buf_1));
#else
                usb_st = write_port(device, send_buf_1, sizeof(send_buf_1));
#endif
                RecState.detail.FinCar_px30 = false;
            } else if (RecState.detail.Slam_Reset) // UpCar_px30,DownCar_px30,FinCar_px30 Slam_Reset
            {
                send_buf_1[4] = Tx2_p30_RB;
                send_buf_1[10] = Alg_Res_Req;
#ifdef NANO_RUN
                usb_st = tcp_fst.nano_tcp_send(send_buf_1, sizeof(send_buf_1));
#else
                usb_st = write_port(device, send_buf_1, sizeof(send_buf_1));
#endif
                RecState.detail.Slam_Reset = false;
            } else if (RecState.detail.Frt_Px30) // FrtCarReq
            {
                send_buf_1[4] = Tx2_p30_RB;
                send_buf_1[10] = FrtCar_Req;
#ifdef NANO_RUN
                usb_st = tcp_fst.nano_tcp_send(send_buf_1, sizeof(send_buf_1));
#else
                usb_st = write_port(device, send_buf_1, sizeof(send_buf_1));
#endif
                RecState.detail.Frt_Px30 = false;
            } else if (RecState.detail.Bck_Px30) // UpCar_px30,DownCar_px30,FinCar_px30 Slam_Reset
            {
                send_buf_1[4] = Tx2_p30_RB;
                send_buf_1[10] = BckCar_Req;
#ifdef NANO_RUN
                usb_st = tcp_fst.nano_tcp_send(send_buf_1, sizeof(send_buf_1));
#else
                usb_st = write_port(device, send_buf_1, sizeof(send_buf_1));
#endif
                RecState.detail.Bck_Px30 = false;
            } else if (RecState.detail.WorkIn) // Work In
            {
                send_buf_1[4] = Tx2_p30_RB;
                send_buf_1[10] = Work_In;
#ifdef NANO_RUN
                usb_st = tcp_fst.nano_tcp_send(send_buf_1, sizeof(send_buf_1));
#else
                usb_st = write_port(device, send_buf_1, sizeof(send_buf_1));
#endif
                RecState.detail.WorkIn = false;
            } else if (RecState.detail.WorkOut) // Work Out
            {
                send_buf_1[4] = Tx2_p30_RB;
                send_buf_1[10] = Work_Out;
#ifdef NANO_RUN
                usb_st = tcp_fst.nano_tcp_send(send_buf_1, sizeof(send_buf_1));
#else
                usb_st = write_port(device, send_buf_1, sizeof(send_buf_1));
#endif
                RecState.detail.WorkOut = false;
            } else if (RecState.detail.Map_Px30) // 建图初始点
            {
                mlog->logInfo("Tx2 back message************Map Init back message");
#ifdef NANO_RUN
                usb_st = tcp_fst.nano_tcp_send(send_data_map, sizeof(send_data_map));
#else
                usb_st = write_port(device, send_data_map, sizeof(send_data_map));
#endif
                RecState.detail.Map_Px30 = false;
            } else if (RecState.detail.Hand_Px30) // 握手
            {
//  usb_st = write_port( device, send_data_hand, sizeof(send_data_hand));
#ifdef NANO_RUN
                usb_st = tcp_fst.nano_tcp_send(send_data_hand, sizeof(send_data_hand));
#else
                usb_st = write_port(device, send_data_hand, sizeof(send_data_hand));
#endif
                mlog->logInfo("Tx2 back message************Hand Shake back message");
                // TMN_PRT(&send_data_hand[0], sizeof(send_data_hand));
                RecState.detail.Hand_Px30 = false;
            } else if (RecState.detail.Update_Px30) // 升级
            {
#ifdef NANO_RUN
                usb_st = tcp_fst.nano_tcp_send(send_data_update, sizeof(send_data_update));
#else
                usb_st = write_port(device, send_data_update, sizeof(send_data_update));
#endif
                RecState.detail.Update_Px30 = false;
                if (0) {
                    cout << "replace********************update" << endl;
                    sleep(5);
                    system((char *)robot_fm_pkg.c_str());
                    cout << "***********************test begin*******************************************************" << endl;
                    cout << "***********************test*******************************************************" << endl;
                    mlog->logInfo("Px30 send messgae::::::::::::::Serial Program:Receiving upgrade file over");
                    string run_update = common_path + "robot_update/update.sh";
                    if (fork() == 0) {
                        execlp(run_update.c_str(), "sh", NULL);
                    }
                }
                mlog->logInfo("Tx2 back message************Update back message");
            } else if (RecState.detail.Ver_Px30) // 发送版本
            {
                mlog->logInfo("Tx2 back message************send version");
                string tx2_ver = "";
                ifstream Soft_Ver(ver_path);
                if (!Soft_Ver) {
                    cout << "Unable to open version" << endl;
                } else {
                    Soft_Ver >> tx2_ver;
                    std::cout << "tx2 version:" << tx2_ver << std::endl;
                }
                uint8_t send_data_ver[10 + 2 + 10] = {0xA4, 0x59, 0x43, 0x04, 0x6c, 0x01, 0x00, 0x00, 0x00, 0x0a, 0x00, 0x0D, 0x0A}; // 0x00 0x00 0x00 0x01 长度为1
                char send_a[10];
                strcpy(send_a, tx2_ver.c_str());
                for (int i_ver = 0; i_ver < 10; i_ver++)
                    send_data_ver[10 + i_ver] = (uint8_t)send_a[i_ver];
                send_data_ver[20] = 0x0D;
                send_data_ver[21] = 0x0A;
#ifdef NANO_RUN
                usb_st = tcp_fst.nano_tcp_send(send_data_ver, sizeof(send_data_ver));
#else
                usb_st = write_port(device, send_data_ver, sizeof(send_data_ver));
#endif
                RecState.detail.Ver_Px30 = false;
            } else if (RecState.detail.px30_mode_ctl) {
                mlog->logInfo("Tx2 back message************Update back contrl mode");
                send_buf_2[4] = Px30_Mode;
                send_buf_2[10] = (Px30_Mode_Contril.state >> 8) & (0xFF);
                send_buf_2[11] = (Px30_Mode_Contril.state) & (0xFF);
                RecState.detail.px30_mode_ctl = false;
                usb_st = tcp_fst.nano_tcp_send(send_buf_2, sizeof(send_buf_2));
            }
        }
    }
}

void SerialPub::LOC_CBK(const std_msgs::UInt8MultiArray::ConstPtr &msg) {
    time_sta.ps_time = true;
    //    cout<<"loc_cbk"<<msg->data.size()<<"   "<<len_loc<<endl;
    if (msg->data.size() == len_loc + 8) {
        PoseSndMutex.lock();
        for (int i = 0; i < len_loc + 8; i++) { // vec_loc size is 12
            vec_loc[i] = msg->data[i];          // suo
        }                                       // for(int i = 0; i < vec.size();i++){printf("%02x ",vec[i]);}printf("\n");
        memcpy(now_pos.bit, vec_loc, 44);
        PosList.push_back(now_pos);
        PoseSndMutex.unlock();
        RecState.detail.Location_bit = true;
    }
}

void SerialPub::PCA_CBK(const std_msgs::UInt8MultiArray::ConstPtr &msg) {
    time_sta.ai_time = true;
    AiSndMutex.lock();
    vec_ai.clear(); // suo
    for (int i = 0; i < 8; i++)
        ObjTime.bit[i] = msg->data[i];
    for (int i = 8; i < msg->data.size(); i++) {
        vec_ai.push_back(msg->data[i]);
    }
    AiSndMutex.unlock();
    RecState.detail.AiCloud_bit = true;
}

void SerialPub::UP_DWN_CBK(const std_msgs::UInt8MultiArray::ConstPtr &msg) {
    vec_updown.clear(); // suo vec_updown
    for (int i = 0; i < msg->data.size(); i++) {
        vec_updown.push_back(msg->data[i]);
    }
    RecState.detail.Updown_bit = true;
}

void SerialPub::LOC_STT_CBK(const std_msgs::UInt8MultiArray::ConstPtr &msg) {
    time_sta.pss_time = true;
    if (msg->data.size() > 3) {
        rcv_alg_slm_state.detail.fst = msg->data[0];
        rcv_alg_slm_state.detail.sec.astate = msg->data[1];
        rcv_alg_slm_state.detail.thd = msg->data[2];
        rcv_alg_slm_state.detail.four = msg->data[3];
    }
    if (rcv_alg_slm_state.detail.fst == 0x02) {
        SlamUint8mltBuf.data[0] = 0x00;
    }
}

void SerialPub::AI_STT_CBK(const std_msgs::UInt8MultiArray::ConstPtr &msg) {
    time_sta.ais_time = true;
    if (msg->data.size() > 1) {
        // ai_state=msg->data[0]<<24+msg->data[1]<<16+msg->data[2]<<8+msg->data[3];
        rcv_ai_state.detail.fst = msg->data[0];
        rcv_ai_state.detail.sec.astate = msg->data[1];
    }
}

void SerialPub::PUB_MSG(void) {
    if (SlamUint8mltBuf.data[1] == 0x01) {
        if (dir_cal > 39 && rcv_alg_slm_state.detail.thd == 0x02 && rcv_alg_slm_state.detail.four == 0x02) {
            SlamUint8mltBuf.data[1] = 0x00; // 时间大于200ms,同时状态都为02
            dir_cal = 0;
        }
        if (dir_cal < 40) {
            dir_cal++;
        }
    }
    SndToAiPub.publish(AiUint8mltBuf);
    SndToSlmPub.publish(SlamUint8mltBuf);
}

bool SerialPub::USB_STT(void) {
    ifstream In_dirStream("/sys/class/udc/3550000.xudc/state");
    string in_ss;
    if (!In_dirStream) {
        cout << "Unable to open serial state" << endl;
        in_ss = "nu";
    } else {
        In_dirStream >> in_ss;
    }
    const char *m_serial = in_ss.c_str();
    if (*m_serial == 0x63)
        return true;
    else
        return false;
}

void SerialPub::DEAL_DATA(void) {
    uint8_t update_buff_[204800];
    ssize_t rtn = 0;
    while (ros::ok()) {
    Rec_Begin:
        while (QueueLength(Q_Save)) {
            while (ros::ok()) {
                DeQueue(Q_Save, &buf_save[0], 1);
                if (buf_save[0] == 0xA4) {
                    frame_buff_[0] = 0xA4;
                    DeQueue(Q_Save, &buf_save[0], 1);
                    if (buf_save[00] == 0x59) {
                        frame_buff_[1] = 0x59;
                        DeQueue(Q_Save, &buf_save[0], 1);
                        if (buf_save[00] == 0x43) {
                            frame_buff_[2] = 0x43;
                            break;
                        }
                    }
                }
            }
            rtn = DeQueue(Q_Save, &buf_save[0], 7);
            if (rtn != 7) {
                mlog->logError("Serial Program:Rec 7 Error");
                TMN_PRT(&buf_save[0], 7);
                goto Rec_Begin;
            }
            memcpy(&frame_buff_[3], &buf_save[0], 7);
            RcvDataLen = frame_buff_[9] + (frame_buff_[8] << 8) + (frame_buff_[7] << 16) + (frame_buff_[6] << 24);
            if (PrtTmn == 4)
                std::cout << "length=" << RcvDataLen << std::endl;
            while (QueueLength(Q_Save) < RcvDataLen + 1) {
                std::this_thread::sleep_for(std::chrono::milliseconds(1));
            }
            rtn = DeQueue(Q_Save, &buf_save[0], RcvDataLen + 2);
            if (rtn != RcvDataLen + 2) {
                mlog->logError("Px30 send messgae::::::::::::::Serial Program:Rec Lgt Error");
                TMN_PRT(&buf_save[0], RcvDataLen + 2);
                goto Rec_Begin;
            }
            memcpy(&frame_buff_[10], &buf_save[0], RcvDataLen + 2);
            if (frame_buff_[RcvDataLen + 12 - 2] != 0x0D || frame_buff_[RcvDataLen + 12 - 1] != 0x0A) {
                mlog->logError("Px30 send messgae::::::::::::::Serial Program:Rec End Error");
                TMN_PRT(&frame_buff_[0], RcvDataLen + 12);
                goto Rec_Begin;
            }
            switch (frame_buff_[4]) {
            case Tx2_upd_sof: // 升级是0x15，回复是0x14
            {
                switch (frame_buff_[5]) {
                case 01: {
                    send_data_update[4] = 0x14;
                    if (1) {
                        std::string temp_up;
                        for (uint8_t i = 4; i < RcvDataLen - 4; i++) {
                            temp_up += buf_save[i];
                        }
                        std::cout << temp_up << std::endl;
                        temp_up = update_path_;
                        // temp_up=update_path_+temp_up;
                        const char *path_up = temp_up.c_str();
                        mode_t f_attrib = S_IRUSR | S_IWUSR | S_IRGRP | S_IWGRP | S_IROTH;
                        up_file = open(path_up, O_WRONLY | O_CREAT | O_TRUNC, f_attrib);
                        if (up_file < 0) {
                            mlog->logError("Px30 send messgae::::::::::::::Serial Program:Function sub 0x15 open update file");
                        }
                        upgrade_length = buf_save[3] + (buf_save[2] << 8) + (buf_save[1] << 16) + (buf_save[0] << 24);
                        mlog->logInfo("Px30 send messgae::::::::::::::Agree to update APP");
                        cout << "upgrade_length=" << upgrade_length << endl;
                        send_data_update[5] = 0x01; // 允许升级
                    } else {
                        mlog->logInfo("Px30 send messgae::::::::::::::Serial Program:Disagree with APP upgrade");
                        send_data_update[5] = 0x02; // 不允许升级
                    }
                    system((char *)robot_fm_pkg.c_str());
                    RecState.detail.Update_Px30 = true;
                } break;
                case 02: {
                    mlog->logInfo("Px30 send messgae::::::::::::::Serial Program:Receiving upgrade file");
                    int offset = 0; // 前几个文件为文件名，需进行偏移，假设偏移为0,最后一位为CRC校验4位
                    memcpy(update_buff_, buf_save + offset, RcvDataLen - offset - 4);
                    // 校验crc
                    uint32_t check_crc = 0, check_CRC;
                    check_crc = crc32_check(&update_buff_[0], RcvDataLen - offset - 4);
                    // https://zhuanlan.zhihu.com/p/256487370
                    check_CRC = buf_save[RcvDataLen - offset - 1] + (buf_save[RcvDataLen - offset - 2] << 8) + (buf_save[RcvDataLen - offset - 3] << 16) + (buf_save[RcvDataLen - offset - 4] << 24);
                    if (check_CRC == check_crc) {
                        send_data_update[4] = 0x14;
                        send_data_update[5] = 0x03;                            // 校验成功
                        write(up_file, update_buff_, RcvDataLen - offset - 4); // buf_save后面多了od oa 读取的是RcvDataLen是否会主动丢掉od oa?
                        upgrade_length = upgrade_length - (RcvDataLen - 4);    //-（每次接收的长度-4）
                    } else {
                        send_data_update[4] = 0x14;
                        send_data_update[5] = 0x04; // 校验失败
                        mlog->logError("Px30 send messgae::::::::::::::Serial Program:Upgrade function sub 0x15 CRC check error");
                    }
                    RecState.detail.Update_Px30 = true;
                    if (upgrade_length < 1) {
                        mlog->logInfo("Px30 send messgae::::::::::::::Serial Program:Receiving upgrade file over");
                        cout << "upgrade_length=" << upgrade_length << endl;
                        string run_update = common_path + "robot_update/update.sh";
                        if (fork() == 0) {
                            execlp(run_update.c_str(), "sh", NULL);
                        }
                    }
                } break;
                case 03: {
                    mlog->logInfo("Px30 send messgae::::::::::::::Serial Program:Start Update");
                    close(up_file);
                } break;
                case 04: {
                    mlog->logInfo("Px30 send messgae::::::::::::::Serial Program:Start Update");
                    close(up_file);
                } break;
                default:
                    mlog->logError("Px30 send messgae::::::::::::::Serial Program:Function sub 0x15 not understand");
                    break;
                }
            } break;
            case M7_tx2: // M7数据->a35
            {
                if (PrtTmn == 4) {
                    MSG_PRINTF("serial recive");
                    TMN_PRT(&frame_buff_[0], 129);
                }
                RTK_IMU_ODOM();
            } break;
            case M7_px3_ai: // m7-px30-ai数据
            {
                if (PrtTmn == 4) {
                    MSG_PRINTF("serial recive");
                    TMN_PRT(&frame_buff_[0], 129);
                }
                RTK_IMU_ODOM();
            } break;
            case PX30_TX2_SYNC_TIME: // 时间同步
            {
                switch (frame_buff_[10]) // 时间同步开始
                {
                case 01: {
                    mlog->logInfo("Px30 send messgae::::::::::::::Time Sync Begin");
                    RecState.detail.sync_time_px30_tx2 = true;
                    sync_send_bit = 1;
                } break;
                case 02: {
                    mlog->logInfo("Px30 send messgae::::::::::::::Time Sync Over"); // 都完成
                } break;
                case 03: {
                    mlog->logInfo("Px30 send messgae::::::::::::::Time Sync Over");
                    RecState.detail.sync_time_px30_tx2 = true;
                    sync_send_bit = 3;
                } break;
                }
            } break;
            case Px30_Mode: {
                printf("rec mode \n");
                printf("rcv :%2x %2x\n", frame_buff_[10], frame_buff_[11]);
                memcpy(&Px30_Mode_Contril.state, &frame_buff_[10], 2);
                if (Px30_Mode_Contril.detail.down_car_key) {
                    mlog->logInfo("Px30 send messgae::::::::::::::Rec Down Car message");
                    SlamUint8mltBuf.data[2] = 0x02; // 下车
                    AiUint8mltBuf.data[1] = 0x02;   // 下车
                    RecState.detail.DownCaring = true;
                }
                if (Px30_Mode_Contril.detail.up_car_key) {
                    mlog->logInfo("Px30 send messgae::::::::::::::Rec Up Car message");
                    SlamUint8mltBuf.data[2] = 0x01;
                    AiUint8mltBuf.data[1] = 0x01; // 上车
                }
                if (Px30_Mode_Contril.detail.finish_up_down_car_key) {
                    mlog->logInfo("Px30 send messgae::::::::::::::Rec Finish UP Down Car message");
                    SlamUint8mltBuf.data[2] = 0x00;
                    AiUint8mltBuf.data[1] = 0x00; // 上下车完成
                    RecState.detail.FinCaring = true;
                }
                if (Px30_Mode_Contril.detail.runnin_back_key) {
                    mlog->logInfo("Px30 send messgae::::::::::::::Rec back Car message");
                    AiUint8mltBuf.data[2] = 0x01;
                }
                if (Px30_Mode_Contril.detail.running_forword_key) {
                    mlog->logInfo("Px30 send messgae::::::::::::::Rec forword Car message");
                    AiUint8mltBuf.data[2] = 0x00;
                }
                if (Px30_Mode_Contril.detail.work_zone_key) {
                    mlog->logInfo("Px30 send messgae::::::::::::::Rec work zone message");
                    AiUint8mltBuf.data[3] = 0x01;
                }
                if (Px30_Mode_Contril.detail.no_work_zone_key) {
                    mlog->logInfo("Px30 send messgae::::::::::::::Rec no work zone message");
                    AiUint8mltBuf.data[3] = 0x00;
                }
                if (Px30_Mode_Contril.detail.low_power_mode) {
                    mlog->logInfo("Px30 send messgae::::::::::::::Rec low power message");
                }
                if (Px30_Mode_Contril.detail.normal_power_mode) {
                    mlog->logInfo("Px30 send messgae::::::::::::::Rec normal power message");
                }
                RecState.detail.px30_mode_ctl = true;
            } break;
            case Tx2_p30_Req: // 请求
            {
                switch (frame_buff_[10]) {
                case Alg_Res_Req: // 算法复位请求
                {
                    // SYS_RESTART_PTP();
                    // SET_SYSTEM_TIME();
                    mlog->logInfo("Px30 send messgae::::::::::::::Slam Reset");
                    SlamUint8mltBuf.data[0] = 0x04;
                    RecState.detail.Slam_Reset = true;
                } break;
                case Alg_Res: {
                    mlog->logInfo("Px30 send messgae::::::::::::::Slam Set");
                    SlamUint8mltBuf.data[0] = 0x00;
                    RecState.detail.Slam_Reset = true;
                } break;
                case Dir_Cal_Req: // 航向校准请求
                {
                    mlog->logInfo("Px30 send messgae::::::::::::::Dir_Calib");
                    SlamUint8mltBuf.data[1] = 0x01;
                    dir_cal = 0x00;
                    RecState.detail.Dir_Cal_bit = true;
                } break;
                case DownCar_Req: // 下车
                {
                    mlog->logInfo("Px30 send messgae::::::::::::::DownCar_require");
                    SlamUint8mltBuf.data[2] = 0x02;
                    AiUint8mltBuf.data[1] = 0x02; // 下车
                    RecState.detail.DownCar_px30 = true;
                    RecState.detail.DownCaring = true;
                } break;
                case UpCar_Req: // 上车
                {
                    mlog->logInfo("Px30 send messgae::::::::::::::UpCar_require");
                    SlamUint8mltBuf.data[2] = 0x01;
                    AiUint8mltBuf.data[1] = 0x01; // 上车
                    RecState.detail.UpCar_px30 = true;
                } break;
                case FinCar_Req: // 上下车完成
                {
                    mlog->logInfo("Px30 send messgae::::::::::::::UpDownCar_finish");
                    SlamUint8mltBuf.data[2] = 0x00;
                    AiUint8mltBuf.data[1] = 0x00; // 上下车完成
                    RecState.detail.FinCar_px30 = true;
                    RecState.detail.FinCaring = true;
                } break;
                case FrtCar_Req: // 前进
                {
                    mlog->logInfo("Px30 send messgae::::::::::::::Front Car_Req");
                    AiUint8mltBuf.data[2] = 0x00;
                    RecState.detail.Frt_Px30 = true;
                } break;
                case BckCar_Req: // 后退
                {
                    mlog->logInfo("Px30 send messgae::::::::::::::Back Car_Req");
                    AiUint8mltBuf.data[2] = 0x01;
                    RecState.detail.Bck_Px30 = true;
                } break;
                case Work_In: {
                    mlog->logInfo("Px30 send messgae::::::::::::::WorkIn_Req");
                    AiUint8mltBuf.data[3] = 0x01;
                    RecState.detail.WorkIn = true;
                } break;
                case Work_Out: {
                    mlog->logInfo("Px30 send messgae::::::::::::::WorkOut_Req");
                    AiUint8mltBuf.data[3] = 0x00;
                    RecState.detail.WorkOut = true;
                } break;
                default: {
                    mlog->logError("Serial Program:PX30->TX2 REQUIRE:0x9a not understand");
                    TMN_PRT(&frame_buff_[0], 89);
                    break;
                }
                }
            } break;
            case Tx2_px3_Hand: // 握手
            {
                mlog->logInfo("Px30 send messgae::::::::::::::Recive Hand Shake message");
                send_data_hand[4] = 0x07;
                send_data_hand[5] = 0x02;
                // TMN_PRT(&frame_buff_[0], 13);
                RecState.detail.Hand_Px30 = true;
                handshake_bit = true;
            } break;
            case Tx2_p30_UI: //UI
            {
                switch (frame_buff_[10]) {
                case 01: {
                    mlog->logInfo("Px30 send messgae::::::::::::::Recive UI message:Open Idle Model");
                    idle_modle = 0x01;
                    SlamUint8mltBuf.data[3] = 0x01;
                    AiUint8mltBuf.data[4] = 0x01; //开启model模式
                } break;
                case 02: {
                    mlog->logInfo("Px30 send messgae::::::::::::::Recive UI message:Close Idle Model");
                    idle_modle = 0x02;
                    SlamUint8mltBuf.data[3] = 0x00;
                    AiUint8mltBuf.data[4] = 0x00; //关闭model模式
                } break;
                }
                RecState.detail.Ui_Px30 = true;
            } break;
            case Tx2_p30_Pic: //pic
            {
                switch (frame_buff_[10]) {
                case 01: {
                    mlog->logInfo("Px30 send messgae::::::::::::::Recive UI message:Pic send");
                } break;
                case 02: {
                    mlog->logInfo("Px30 send messgae::::::::::::::Recive UI message:Pic stop");
                } break;
                }
                RecState.detail.Send_Pic = true;
            } break;
            case Bld_map_init: // build map init point 接收0x09，回复0x08
            {
                switch (frame_buff_[10]) {
                case 01: {
                    mlog->logInfo("Px30 send messgae::::::::::::::Recive mapping init point 0x09 0x01");
                    printf("************\n");
                    send_data_map[4] = Bld_map_bck;
                    send_data_map[5] = 0x01;
                    send_data_map[10] = 0x01;
                    map_service++;
                    if (rtk_x == 0) {
                        send_data_map[5] = 0x02;
                        mlog->logInfo("No rtk message from m7");
                    } else {
                        send_data_map[5] = 0x01;
                        mlog->logInfo("Mapping init point");
                    }
                    *(double *)&send_data_map[11] = rtk_x;
                    *(double *)&send_data_map[19] = rtk_y;
                    *(float *)&send_data_map[27] = rtk_z;
                    TMN_PRT(&frame_buff_[0], 13);
                    std::ofstream Out_dirStream;
                    Out_dirStream.open(rtk_path_, std::ios::out);
                    Out_dirStream.setf(std::ios::fixed, std::ios::floatfield);
                    Out_dirStream.precision(9);
                    Out_dirStream << rtk_x << " "
                                  << rtk_y << " "
                                  << rtk_z << std::endl;
                    Out_dirStream.close();
                    RecState.detail.Map_Px30 = true;
                } break;
                case 02: {
                    mlog->logInfo("Px30 send messgae::::::::::::::Recive mapping init point 0x09 0x02");
                    send_data_map[4] = 0x08;
                    send_data_map[5] = 0x01;
                    send_data_map[10] = 0x02;
                    map_service++;
                    double change_point_x = 0.00, change_point_y = 0.00;
                    if (RcvDataLen > 16) {
                        memcpy(&change_point_x, &frame_buff_[11], 8);
                        memcpy(&change_point_y, &frame_buff_[19], 8);
                        std::ofstream Out_dirStream;
                        Out_dirStream.open(rtk_path_, std::ios::out);
                        Out_dirStream.setf(std::ios::fixed, std::ios::floatfield);
                        Out_dirStream.precision(9);
                        Out_dirStream << change_point_x << " "
                                      << change_point_y << " "
                                      << 1.00 << std::endl;
                        std::cout << "change_point_x= " << change_point_x << "change_point_y= " << change_point_y << std::endl;
                        Out_dirStream.close();
                        RecState.detail.Map_Px30 = true;
                    } else {
                        std::cout << "change_point length too short,the length is" << RcvDataLen << std::endl;
                    }
                } break;
                default: {
                    mlog->logError("Serial Program:Mapping init point,0x09 not understand");
                    TMN_PRT(&frame_buff_[0], 89);
                    break;
                }
                }
            } break;
            case p30_Tx2_log: // 传LOG
            {
                switch (frame_buff_[5]) {
                case 01:
                    mlog->logInfo("Px30 send messgae::::::::::::::Tx2 p30 log mode 1");
                    RecState.detail.Loging_Px30 = true;
                    log_mode = 1;
                    break;
                case 02:
                    mlog->logInfo("Px30 send messgae::::::::::::::Px30 recive log sucess");
                    break;
                case 03:
                    mlog->logError("Px30 send messgae::::::::::::::Px30 recive log fail");
                    break;
                case 04:
                    mlog->logInfo("Px30 send messgae::::::::::::::Tx2 p30 log mode 2");
                    RecState.detail.Loging_Px30 = true;
                    log_mode = 2;
                default:
                    break;
                }
            } break;
            case Tx2_Soft_V: // 版本请求
            {
                mlog->logInfo("Px30 send messgae::::::::::::::Tx2 Software Vision");
                switch (frame_buff_[5]) {
                case 0x01:
                    RecState.detail.Ver_Px30 = true;
                    break;
                default:
                    break;
                }
            } break;
            default: {
                mlog->logError("Px30 send messgae::::::::::::::Serial Program:Function main not understand");
                TMN_PRT(&frame_buff_[0], 89);
                break;
            }
            }
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(1));
    }
}
void SerialPub::DATA_RCV() {
    QElemType rcv_buf[10240]; // 10k
    mlog->logInfo("Serial Program:Data reception");
    serial_tbefore = (ros::Time::now()).toSec();
    double serial_tnow = 0;
    int rcv_ser = 0;
    while (ros::ok()) {
#ifdef NANO_RUN
        rcv_ser = tcp_fst.nano_tcp_recive(rcv_buf, sizeof(rcv_buf));
#else
        rcv_ser = read(device, rcv_buf, sizeof(rcv_buf));
#endif
        usb_st = rcv_ser;
        if (rcv_ser > 0) {
            if (!EnQueue(Q_Save, &rcv_buf[0], rcv_ser)) {
                printf("(%d,%d)\n", Q_Save.front, Q_Save.rear);
            }
            serial_tnow = (ros::Time::now()).toSec();
            one_secend_count.rec_tik = 0;
        }
        if (serial_tnow - serial_tbefore > 4) {
            mlog->logError("Serial Program:USB data recover");
        }
        serial_tbefore = serial_tnow;
        std::this_thread::sleep_for(std::chrono::milliseconds(2));
    }
}

void SerialPub::SYS_STT() {
    while (ros::ok()) {
        std::this_thread::sleep_for(std::chrono::milliseconds(20));
        one_secend_count.header_tik++;
        one_secend_count.imu_tik++;
        one_secend_count.obj_tik++;
        one_secend_count.odom_tik++;
        one_secend_count.pos_tik++;
        one_secend_count.rtk_tik++;
        one_secend_count.temp_tik++;
        one_secend_count.rec_tik++;
        one_secend_count.error_tik++;
        if (one_secend_count.error_tik > 49) {
            one_secend_count.error_tik = 0;
            mlog_save_tik.detail.error = true;
        }
        if (one_secend_count.header_tik > 49) {
            one_secend_count.header_tik = 0;
            RecState.detail.Hand_Px30 = true;
        }
        if (one_secend_count.imu_tik > 49) {
            one_secend_count.imu_tik = 0;
            mlog_save_tik.detail.imu = true;
        }
        if (one_secend_count.odom_tik > 49) {
            one_secend_count.odom_tik = 0;
            mlog_save_tik.detail.odom = true;
        }
        if (one_secend_count.rtk_tik > 49) {
            one_secend_count.rtk_tik = 0;
            mlog_save_tik.detail.rtk = true;
        }
        if (one_secend_count.obj_tik > 49) {
            one_secend_count.obj_tik = 0;
            mlog_save_tik.detail.object = true;
        }
        if (one_secend_count.pos_tik > 49) {
            one_secend_count.pos_tik = 0;
            mlog_save_tik.detail.position = true;
        }
        if (one_secend_count.temp_tik > 49) {
            one_secend_count.temp_tik = 0;
            mlog->logInfo("tempr" + to_string(cpu_tempr_get(0)) + "," + to_string(cpu_tempr_get(1)) + "," + to_string(cpu_tempr_get(2)));
        }
        if (sync_send_bit == 1) {
            sync_success_bit = timesync_client_time_sync_ack_get();
            if (sync_success_bit != last_sync_bit) {
                if (sync_success_bit == 0) // 时间同步成功,开始相机同步
                {
                    mlog->logInfo("sync time mode change to -1,begin camera sync");
                    timesync_sync_pulse_output_enable(1, 1);
                    all_sync_bit = 1; // 开始相机同步
                    timeval cam_sync_begin_time;
                    gettimeofday(&cam_sync_begin_time, NULL); // 记录时间
                    cam_sync_before = cam_sync_begin_time.tv_sec + cam_sync_begin_time.tv_usec / 1000000.00;
                }
                if (sync_success_bit < 0) {
                    mlog->logInfo("sync time mode change to -1");
                    all_sync_bit = -1; // 未进行时间同步
                }
            }
            if (all_sync_bit == 1) {
                timeval cam_sync_end_time;
                gettimeofday(&cam_sync_end_time, NULL);
                cam_sync_now = cam_sync_end_time.tv_sec + cam_sync_end_time.tv_usec / 1000000.00;
                if (cam_sync_now - cam_sync_before > 1.5) {
                    mlog->logInfo("sync time camera sync over");
                    all_sync_bit = 2;                          // 如果相机同步时间>1.5s,相机同步完成
                    RecState.detail.sync_time_px30_tx2 = true; // 发送成功
                    sync_send_bit = 2;
                }
            }
        }
        last_sync_bit = sync_success_bit;

        if (rcv_ai_state.detail.sec.ibit.ucA == 0x01 || rcv_alg_slm_state.detail.sec.ibit.ucD == 0x01) // camera
        {
            // alg_error_time_.detail.camera_++;
            // if (alg_error_time_.detail.camera_ > 2)
            //{
            tx2_error.detail.u0.ucA = 0x01;
            mlog->logInfo("Sensor Camera Error");
            //}
        } else {
            tx2_error.detail.u0.ucA = 0x00;
            // alg_error_time_.detail.camera_ = 0x00;
        }

        if (rcv_ai_state.detail.sec.ibit.ucB == 0x01 || rcv_alg_slm_state.detail.sec.ibit.ucE == 0x01) // lidar
        {
            // alg_error_time_.detail.lidar_++;
            // if (alg_error_time_.detail.lidar_ > 2)
            //{
            tx2_error.detail.u0.ucB = 0x01;
            mlog->logInfo("Sensor Lidar Error");
            // }
        } else {
            tx2_error.detail.u0.ucB = 0x00;
            // alg_error_time_.detail.lidar_ = 0x00;
        }

        tx2_error.detail.u0.ucC = rcv_ai_state.detail.sec.ibit.ucC;      // realsense
        tx2_error.detail.u0.ucE = rcv_alg_slm_state.detail.sec.ibit.ucA; // imu
        tx2_error.detail.u0.ucF = rcv_alg_slm_state.detail.sec.ibit.ucB; // odom
        tx2_error.detail.u0.ucG = rcv_alg_slm_state.detail.sec.ibit.ucC; // rtk
        tx2_error.detail.u0.ucH = rcv_alg_slm_state.detail.sec.ibit.ucF; // viw
        tx2_error.detail.u1.ucA = rcv_alg_slm_state.detail.sec.ibit.ucG; // lio
        tx2_error.detail.u1.ucB = rcv_alg_slm_state.detail.sec.ibit.ucH; // fusion
        tx2_error.detail.u1.ucC = rcv_ai_state.detail.sec.ibit.ucD;      // ai
        tx2_error.detail.u1.ucD = rcv_ai_state.detail.sec.ibit.ucE;      // points cloud
        tx2_error.detail.u1.ucE = rcv_ai_state.detail.sec.ibit.ucF;      // ai+points
        if (mlog_save_tik.detail.error) {
            if (rcv_alg_slm_state.detail.sec.ibit.ucA)
                mlog->logInfo("Alg imu Error");
            if (rcv_alg_slm_state.detail.sec.ibit.ucB)
                mlog->logInfo("Alg odom Error");
            if (rcv_alg_slm_state.detail.sec.ibit.ucC)
                mlog->logInfo("Alg rtk Error");
            if (rcv_alg_slm_state.detail.sec.ibit.ucD)
                mlog->logInfo("Alg ai Error");
            if (rcv_alg_slm_state.detail.sec.ibit.ucE)
                mlog->logInfo("Alg points cloud Error");
            if (rcv_alg_slm_state.detail.sec.ibit.ucF)
                mlog->logInfo("Alg viw Error");
            if (rcv_alg_slm_state.detail.sec.ibit.ucG)
                mlog->logInfo("Alg lio Error");
            if (rcv_alg_slm_state.detail.sec.ibit.ucH)
                mlog->logInfo("Alg fusion Error");
            mlog_save_tik.detail.error = false;
        }
        /*if ((cpu_tempr_get(0) > 75000) || (cpu_tempr_get(1) > 75000) || (cpu_tempr_get(2) > 75000))
            tx2_error.detail.u1.ucF = 0x00; //  ai_error|=1<<14;
        else
            tx2_error.detail.u1.ucF = 0x00; //  ai_error&=~(1<<14);
        if (disk_usage() > 95)
            tx2_error.detail.u1.ucG = 0x00; //  ai_error|=1<<15;
        else
            tx2_error.detail.u1.ucG = 0x00;*/
        one_secend_count.rec_tik++;
#ifdef NANO_RUN
        if ((one_secend_count.rec_tik > 9)) {
            mlog->logError("Serial Program:DEVICE NO DATA");
        }
#else
        UsbStt = USB_STT(); // 100ms
        if ((one_secend_count.rec_tik > 9) && (UsbStt)) {
            mlog->logError("Serial Program:DEVICE NO DATA");
        }
        if (!UsbStt) {
            mlog->logError("Serial Program:DEVICE NO CONNECTION");
            usb_st = -1;
        }
        if (usb_st == -1) {
            close(device);
            Serial_open(UsbName.c_str());
        }
#endif
    }
}

void SerialPub::PPS_RCV() {
    mlog->logInfo("Serial Program:pps signal reception");
    pps_handle_t handle[4];
    int avail_mode[4];
    int ret;
    std::string pps_str = "/dev/pps0";
    char dev_pps[20];
    pps_str.copy(dev_pps, 8, 0);
    *(dev_pps + 9) = '\0';
    strcpy((dev_pps), pps_str.c_str());
open_pps:
    ret = find_source(dev_pps, &handle[0], &avail_mode[0]); // check pps source is opened, set /dev/pps0
    if (ret < 0) {
        std::this_thread::sleep_for(std::chrono::milliseconds(2000));
        mlog->logError("Serial Program:pps disconnection");
        goto open_pps;
    }
    // printf("ok, found %d source(s), now start fetching data...\n",0);
    while (ros::ok()) {
        ret = fetch_source(0, &handle[0], &avail_mode[0]); // support check a lot pps source  block
        if (ret < 0 && errno != ETIMEDOUT)
            exit(EXIT_FAILURE);
        if (ret < 0)
            mlog->logError("Serial Program:pps connection timed out");
        if (ret == 0) {
            pps_bit = true;
        }
    }
    time_pps_destroy(handle[0]); /**/
}

void SerialPub::RTK_IMU_ODOM() {
    handshake_bit = true;
    std::vector<uint8_t> vec_data_rcv(frame_buff_, frame_buff_ + FRAME_LEN);
    std::vector<uint8_t>::iterator match_TIME_H = std::search(vec_data_rcv.begin(), vec_data_rcv.end(), MSG_H_TIME, MSG_H_TIME + 3);

    if (match_TIME_H != vec_data_rcv.end())
        DEAL_TIME(match_TIME_H);
    else
        mlog->logError("Serial Program:find time false");

    std::vector<uint8_t>::iterator match_GPS_H = std::search(vec_data_rcv.begin(), vec_data_rcv.end(), MSG_H_GPS, MSG_H_GPS + 3);
    if (match_GPS_H != vec_data_rcv.end())
        DEAL_GPS(match_GPS_H);
    else
        mlog->logError("Serial Program:find rtk false");

    std::vector<uint8_t>::iterator match_IMU_H = std::search(vec_data_rcv.begin(), vec_data_rcv.end(), MSG_H_IMU, MSG_H_IMU + 3);
    if (match_IMU_H != vec_data_rcv.end())
        DEAL_IMU(match_IMU_H);
    else
        mlog->logError("Serial Program:find imu false");

    std::vector<uint8_t>::iterator match_ODOM_H = std::search(vec_data_rcv.begin(), vec_data_rcv.end(), MSG_H_ODOM, MSG_H_ODOM + 3);
    if ((match_ODOM_H != vec_data_rcv.end()))
        DEAL_ODOM(match_ODOM_H);
    else
        mlog->logError("Serial Program:find odom false");

    vec_data_rcv.clear();
}

void SerialPub::PUB_MSG_MAIN() {
    mlog->logInfo("Serial Program:Parsing data");
    ros::Rate loop_rate(200); // 设置发送数据的频率为50Hz 20ms
    while (ros::ok()) {
        PUB_MSG();
        ros::spinOnce();
        loop_rate.sleep();
    }
}

bool SerialPub::DEAL_TIME(std::vector<uint8_t>::iterator match_TIME_) {
    const TIME_frame_t *Time_frame_p = (const TIME_frame_t *)&*(match_TIME_ + 3);
    if (PrtTmn == 6) {
        printf("%d/%d\n", Time_frame_p->Sec, Time_frame_p->MsSec);
        MSG_STATE(0, "time");
        printf("\n");
    }
    time_daysec = Time_frame_p->Sec;
    time_daynsec = Time_frame_p->MsSec * 1000;
    if (pps_bit) {
        timeval stime;
        stime.tv_sec = time_daysec;
        stime.tv_usec = time_daynsec / 1000;
        if (stime.tv_usec > 500000) {
            stime.tv_sec++;
            stime.tv_usec = 0;
        } else {
            stime.tv_usec = 0;
        }
        settimeofday(&stime, NULL);
        pps_bit = false;
    }
}

float latstd = 0, lonstd = 0, latvar = 0, lonvar = 0;
uint32_t gps_time_sec = 0, gps_time_usec = 0;
int levela = 0;
bool SerialPub::DEAL_GPS(std::vector<uint8_t>::iterator match_GPS_) {
    const GPS_frame_t *GPS_frame_p = (const GPS_frame_t *)&*(match_GPS_ + 3);
    const GPS_frame_t1 *GPS_frame_p1 = (const GPS_frame_t1 *)&*(match_GPS_ + 32);
    gps_time_sec = GPS_frame_p->sec;
    gps_time_usec = GPS_frame_p->usec;
    latstd = GPS_frame_p1->lat_std;
    lonstd = GPS_frame_p1->lon_std;
    levela = (int)GPS_frame_p->level;
    latvar = latstd * latstd;
    lonvar = lonstd * lonstd;
    if (time_system) {
        sensor_time = ros::Time::now();
        FixMsg.header.stamp = sensor_time;
    } else {
        // ros::Time sensor_time1(time_daysec, time_daynsec);
        ros::Time sensor_time1(gps_time_sec, gps_time_usec * 1000);
        FixMsg.header.stamp = sensor_time1;
    }
    FixMsg.status.status = levela;
    FixMsg.status.service = map_service;
    FixMsg.longitude = GPS_frame_p->lon;
    rtk_x = GPS_frame_p->lat;
    FixMsg.latitude = GPS_frame_p->lat;
    rtk_y = GPS_frame_p->lon;
    FixMsg.altitude = GPS_frame_p->hgt;
    rtk_z = GPS_frame_p->hgt;
    FixMsg.position_covariance = {lonvar, 0, 0,
                                  0, latvar, 0,
                                  0, 0, 0};
    FixMsg.header.frame_id = "GPS_fix";
    FixGpsPub.publish(FixMsg);
    if (mlog_save_tik.detail.rtk) {
        mlog->logInfo("RTK:" + to_string(rtk_x) + "," + to_string(rtk_y));
        mlog_save_tik.detail.rtk = false;
    }
    return true;
}

double time_record[4];
bool SerialPub::DEAL_IMU(std::vector<uint8_t>::iterator match_IMU_) {
    const IMU_frame_t *IMU_frame_p = (const IMU_frame_t *)&*(match_IMU_ + 3);
    if (time_system) {
        sensor_time = ros::Time::now();
        SensImu.header.stamp = sensor_time;
    } else {
        ros::Time sensor_time1(time_daysec, time_daynsec);
        SensImu.header.stamp = sensor_time1;
    }
    time_record[0] = SensImu.header.stamp.toSec();
    time_record[1] = ros::Time::now().toSec();
    if (time_record[1] - time_record[2] > 0.1) {
        mlog->logError((string) "Imu time out: " + (string) "sys ntime :" + to_string(time_record[1]) + (string) "sys btime :" + to_string(time_record[2]) + (string) "imu ntime :" + to_string(time_record[0]) + (string) "imu btime :" + to_string(time_record[3]));
    }
    time_record[2] = time_record[1];
    time_record[3] = time_record[0];
    SensImu.header.frame_id = "imu_link";
    SensImu.angular_velocity.x = IMU_frame_p->gyro_x * pi / 180.00;
    SensImu.angular_velocity.y = IMU_frame_p->gyro_y * pi / 180.00;
    SensImu.angular_velocity.z = IMU_frame_p->gyro_z * pi / 180.00;

    SensImu.linear_acceleration.x = IMU_frame_p->acc_x;
    SensImu.linear_acceleration.y = IMU_frame_p->acc_y;
    SensImu.linear_acceleration.z = IMU_frame_p->acc_z;

    SensImu.orientation.x = IMU_frame_p->raw;
    SensImu.orientation.y = IMU_frame_p->pich;
    SensImu.orientation.z = IMU_frame_p->yaw;

    if (ImuBit) {
        double serial_tnow = (ros::Time::now()).toSec();
        if (serial_tnow - imu_last_time > 0.015 || serial_tnow - imu_last_time < 0.005) {
            Imu_Pose_dirStream << serial_tnow << "time small or big" << std::endl;
        } else
            Imu_Pose_dirStream << serial_tnow << std::endl;
        imu_last_time = serial_tnow;
    }
    if (mlog_save_tik.detail.imu) {
        mlog->logInfo("IMU: " + to_string(IMU_frame_p->acc_x) + "," + to_string(IMU_frame_p->acc_y) + "," + to_string(IMU_frame_p->acc_z));
        mlog_save_tik.detail.imu = false;
    }
    SensImuPub.publish(SensImu);
}

uint32_t right_enc = 0, left_enc = 0;
float speed_end = 0, speed_endth = 0, distance_sum = 0, distance_diff = 0, Yaw_Angle_Diff = 0, Yaw_Angle = 0, Yaw_Angle_1 = 0;
bool SerialPub::DEAL_ODOM(std::vector<uint8_t>::iterator match_ODOM_) {
    const ODOM_frame_t *ODOM_frame_p = (const ODOM_frame_t *)&*(match_ODOM_ + 3);
    right_enc = ODOM_frame_p->right_encode; // 10ms encoder_num
    left_enc = ODOM_frame_p->left_encode;
    speed_end = (ODOM_frame_p->right_encode + ODOM_frame_p->left_encode) / 2.0 * const_frame / dt; //(vr+vl)/2
    speed_endth = (ODOM_frame_p->right_encode - ODOM_frame_p->left_encode) * const_angle / dt;     // m/s (vr-vl)/l
    distance_sum = speed_end * dt;                                                                 // 短时间内，前进距离两侧速度和 delta_dis=speed*delta_dt
    distance_diff = speed_endth * dt;                                                              // 短时间内，转弯角度为两侧速度差
    Yaw_Angle_Diff = distance_diff;                                                                // 采样期间走的角度
    Yaw_Angle = Yaw_Angle + Yaw_Angle_Diff;                                                        // 计算里程计方向角
    Yaw_Angle_1 = Yaw_Angle + 0.5f * Yaw_Angle_Diff;                                               // 里程计方向角数据位数变化，用于三角函数计算

    odom_X = odom_X + distance_sum * cos(Yaw_Angle_1); // 距离*Yaw（偏角） 解析X坐标
    odom_Y = odom_Y + distance_sum * sin(Yaw_Angle_1);
    if (Yaw_Angle > pi) {
        Yaw_Angle -= 2 * pi;
    } else if (Yaw_Angle < -pi) {
        Yaw_Angle += 2 * pi;
    }
    if (PrtTmn == 4) {
        MSG_PRINTF_FLOAT("ANGLE DATA: %f", Yaw_Angle);
        MSG_PRINTF_FLOAT("SPEED DATA %f", speed_end);
        MSG_PRINTF_FLOAT("ODOM X: %f", odom_X);
        MSG_PRINTF_FLOAT("ODOM Y: %f", odom_Y);
    }
    if (time_system) {
        sensor_time = ros::Time::now();
        TwsOdom.header.stamp = sensor_time;
    } else {
        ros::Time sensor_time1(time_daysec, time_daynsec);
        TwsOdom.header.stamp = sensor_time1;
    }
    TwsOdom.header.frame_id = "wheel_odom";
    TwsOdom.twist.linear.x = speed_end;
    TwsOdom.twist.linear.y = 0;
    TwsOdom.twist.linear.z = 0; // Anglev_send;//角速度
    TwsOdom.twist.angular.x = 0;
    TwsOdom.twist.angular.y = 0;
    TwsOdom.twist.angular.z = speed_endth; // Anglev_send;//角速度
    TwsOdomPub.publish(TwsOdom);
    if (mlog_save_tik.detail.odom) {
        mlog->logInfo("ODOM:" + to_string(speed_end) + "," + to_string(speed_endth));
        mlog_save_tik.detail.odom = false;
    }
}
